#include <p32xxxx.h>
#include "FreeRTOSConfig.h"
#include "FreeRTOS.h"
#include "main_peripherals.h"

void BUZZ(int delay, int beeps)
{
	while (beeps-- > 0)
	{
		BUZZ_ON;
		vTaskDelay(delay);
		BUZZ_OFF;
		vTaskDelay(delay);
	}
}

void peripheralsInit()
{
	// all digital.
	AD1PCFG = 0xFFFF;

	{ /* Digital outputs [leds, buzzer, etc] */
        TRISECLR =  _TRISE_TRISE0_MASK | // RE0: LED1
                    _TRISE_TRISE1_MASK | // RE1: LED2
                    _TRISE_TRISE2_MASK | // RE2: OLED
                    _TRISE_TRISE3_MASK | // RE3: GLED
                    _TRISE_TRISE4_MASK;  // RE4: WLED
        TRISGCLR =  _TRISG_TRISG12_MASK | // G12: LED4
                    _TRISG_TRISG13_MASK | // G13: BLED
                    _TRISG_TRISG14_MASK;  // G14: LED3
        TRISACLR =  _TRISA_TRISA9_MASK |  // A9: BUZZ
                    _TRISA_TRISA0_MASK;   // A0: RLED
        TRISFCLR =  _TRISF_TRISF8_MASK; // RF8: BENCH DEBUG / RX-IN
	}

	/**
	 * PWM outputs
	 */
	/*{
		// Use Timer2 as OC source
		// TODO: Fix values / postscaler
		OpenTimer2(	T2_ON | T2_IDLE_CON | T2_GATE_OFF | T2_PS_1_32, 1000);

		// Timer2 IRQ for software PWM's
		ConfigIntTimer2(T2_INT_PRIOR_7 | T2_INT_SUB_PRIOR_0 | T2_INT_ON);
		INTEnable(INT_T2, 1);

		// Timer4 IRQ for software PWM's (intermediate intervals for pin toggling)
		OpenTimer4(T4_ON | T4_IDLE_CON | T4_GATE_OFF | T4_PS_1_4, 0xFFFF);
		ConfigIntTimer4(T4_INT_PRIOR_7 | T4_INT_SUB_PRIOR_1 | T4_INT_OFF);
	}*/


	{ /* I2C1 (ESC's) / I2C2 (sensors) */
        I2C1BRG = configPERIPHERAL_CLOCK_HZ / (800000 * 2) - 2;
        I2C1CON = _I2C1CON_ON_MASK;
        IPC6bits.I2C1IP = 6;

        I2C2BRG = configPERIPHERAL_CLOCK_HZ / (400000 * 4) - 2;
        I2C2CON = _I2C2CON_ON_MASK;
        IEC1SET = _IEC1_I2C2MIE_MASK | _IEC1_I2C2BIE_MASK;
        IPC8bits.I2C2IP = 6;
	}

	/**
	 * SD / SPI
	 */
	{
		// SDO/CLK/CS
//		PORTSetPinsDigitalOut	(IOPORT_D,BIT_0|BIT_9|BIT_10);

        TRISDCLR =  _TRISD_TRISD0_MASK |
                    _TRISD_TRISD9_MASK |
                    _TRISD_TRISD10_MASK;
		// SDI
//		PORTSetPinsDigitalIn	(IOPORT_C,BIT_4);
        TRISDSET =  _TRISD_TRISD4_MASK;

        SPI1BRG = configPERIPHERAL_CLOCK_HZ / (2 * 250000) - 1;
        SPI1CON =   _SPI1CON_CKP_MASK |
                    _SPI1CON_SMP_MASK |
                    _SPI1CON_MSTEN_MASK |
                    _SPI1CON_ON_MASK;

//		SpiChnConfigure			(SPI_SD, SPI_CONFIG_MSTEN|SPI_CONFIG_MODE8|SPI_CONFIG_CKE_REV|SPI_CONFIG_SMP_END);
//		SpiChnSetBitRate		(SPI_SD, configPERIPHERAL_CLOCK_HZ, 250000); // low speed SD init
//		SpiChnEnable			(SPI_SD, SPI_ENABLE);

        DCH5CONSET = (3 << _DCH5CON_CHPRI_POSITION);
        DCH5ECON =   _DCH5ECON_SIRQEN_MASK |
                     (_SPI1_TX_IRQ << _DCH5ECON_CHSIRQ_POSITION);

        //DmaChnOpen				(DMA_SD_TX, DMA_CHN_PRI3, DMA_OPEN_DEFAULT);
		//DmaChnSetEventControl	(DMA_SD_TX, DMA_EV_START_IRQ_EN|DMA_EV_START_IRQ(SPI_SD_TX_IRQ));

        DCH0CONSET = (3 << _DCH0CON_CHPRI_POSITION);
        DCH0ECON =   _DCH0ECON_SIRQEN_MASK |
                     (_SPI1_RX_IRQ << _DCH0ECON_CHSIRQ_POSITION);
//		DmaChnOpen				(DMA_SD_RX, DMA_CHN_PRI3, DMA_OPEN_DEFAULT);
//		DmaChnSetEventControl	(DMA_SD_RX, DMA_EV_START_IRQ_EN|DMA_EV_START_IRQ(SPI_SD_RX_IRQ));


        IPC10bits.DMA5IP = 4;
        IPC10bits.DMA5IS = 2;

        IPC9bits.DMA0IP = 4;
        IPC9bits.DMA0IS = 3;
//		INTSetVectorPriority	(INT_VECTOR_DMA(DMA_SD_TX), PRIORITY_SD_TX);
//		INTSetVectorSubPriority	(INT_VECTOR_DMA(DMA_SD_TX), INT_SUB_PRIORITY_LEVEL_2);
//		INTSetVectorPriority	(INT_VECTOR_DMA(DMA_SD_RX), PRIORITY_SD_RX);
//		INTSetVectorSubPriority	(INT_VECTOR_DMA(DMA_SD_RX), INT_SUB_PRIORITY_LEVEL_3);
	}

	{ /* Spektrum RC */
        U4BRG = configPERIPHERAL_CLOCK_HZ / (4 * 115200) - 1;
        U4STA =     _U4STA_URXEN_MASK;
        U4MODE =    _U4MODE_BRGH_MASK |
                    _U4MODE_ON_MASK;

        IEC2SET =   _IEC2_U1BEIE_MASK |
                    _IEC2_U1BRXIE_MASK;

        IPC12bits.U1BIP = 3;
	}

	{ /** BLUETOOTH */

		//UARTConfigure			(UART_BT, UART_ENABLE_PINS_CTS_RTS | UART_ENABLE_HIGH_SPEED | UART_RTS_WHEN_RX_NOT_FULL);
        U2BRG = configPERIPHERAL_CLOCK_HZ / (4 * 115200 * 8); //- 2;

        //UARTSetDataRate			(UART_BT, configPERIPHERAL_CLOCK_HZ, 115200*8);

        U2STA =     _U2STA_UTXISEL1_MASK |
                    _U2STA_UTXEN_MASK |
                    _U4STA_URXEN_MASK;
                    //_U2STA_URXISEL1_MASK;

        U2MODE =    _U2MODE_UEN1_MASK |
                    _U2MODE_BRGH_MASK |
                    _U2MODE_ON_MASK;

        //UARTSetLineControl		(UART_BT, UART_DATA_SIZE_8_BITS | UART_PARITY_NONE | UART_STOP_BITS_1);
//		UARTSetFifoMode			(UART_BT, UART_INTERRUPT_ON_TX_DONE | UART_INTERRUPT_ON_RX_NOT_EMPTY);
//		UARTEnable				(UART_BT, UART_ENABLE_FLAGS(UART_PERIPHERAL | UART_RX | UART_TX));

		//INTEnable				(INT_SOURCE_UART_RX(UART_BT), INT_ENABLED);
//		INTSetVectorPriority	(INT_VECTOR_UART(UART_BT), PRIORITY_BT);
//	    INTSetVectorSubPriority	(INT_VECTOR_UART(UART_BT), INT_SUB_PRIORITY_LEVEL_0);

        TRISBCLR =  _TRISB_TRISB15_MASK; // RB15: RN RESET
//		PORTSetPinsDigitalOut(IOPORT_B, BIT_15); // RB15 => Reset

#ifndef __DEBUG
        BLUETOOTH_RESET_ON;
		vTaskDelay(configTICK_RATE_HZ / 10);
		BLUETOOTH_RESET_OFF;
#endif
		/*vTaskDelay(configTICK_RATE_HZ * 10);
		unsigned char b = 0x00;
		for(;;)
		{
			int i;
			
			for (i=0;i<128;i++)
			{
				while (!UARTTransmitterIsReady(UART_BT));
				//if (i == 0) UARTSendDataByte(UART_BT,10);
//				else if (i == 1) UARTSendDataByte(UART_BT, ((xTaskGetTickCount() % 1000)/100) + '0');
//				else if (i == 2) UARTSendDataByte(UART_BT, ((xTaskGetTickCount() % 100)/10) + '0');
//				else if (i == 3) UARTSendDataByte(UART_BT, ((xTaskGetTickCount() % 10)) + '0');
//				else if (i == 4) UARTSendDataByte(UART_BT, ' ');
//				else if (i == 1) UARTSendDataByte(UART_BT,13);
//				else UARTSendDataByte(UART_BT, b);
				UARTSendDataByte(UART_BT, b);
				while (UARTReceivedDataIsAvailable(UART_BT))
				{
					UARTGetDataByte(UART_BT);
				}
			}


			b++;

			//vTaskDelay(1); //configTICK_RATE_HZ);
		}*/
	}


	/**
	 * GPS UART
	 */
	/*{
		UARTSetDataRate			(UART_GPS, configPERIPHERAL_CLOCK_HZ, 115200);
		UARTConfigure			(UART_GPS, UART_ENABLE_PINS_TX_RX_ONLY);
		UARTSetLineControl		(UART_GPS, UART_DATA_SIZE_8_BITS | UART_PARITY_NONE | UART_STOP_BITS_1);
		//UARTSetFifoMode		(GPS_UART, UART_INTERRUPT_ON_TX_DONE | UART_INTERRUPT_ON_RX_HALF_FULL);
		UARTEnable				(UART_GPS, UART_ENABLE_FLAGS(UART_PERIPHERAL | UART_TX | UART_RX));

		//gpsStartDMAReceive();
	}*/

	{ /* Sensor interrupts */
		// INT1/RE8 : DRDY (HMC)
		// INT2/RE9 : INTA (MPU)
//		PORTSetPinsDigitalIn	(IOPORT_E, BIT_8 | BIT_9);
        TRISESET =  _TRISE_TRISE8_MASK | // INT1/RE8: DRDY (HMC)
                    _TRISE_TRISE9_MASK;  // INT2/RE9: INTA (MPU)
	}

	{ /* ADC Vlipo / current */
		// set as analog inputs
		// RB0 (lipo) / RB1 (current) -> AN0 / AN1
		AD1PCFGbits.PCFG0 = 0;
		AD1PCFGbits.PCFG1 = 0;

		AD1CON1bits.FORM = 4; // unsigned 32 bit int
		AD1CON1bits.SSRC0 = 1; // auto convert
		AD1CON1bits.SSRC1 = 1;
		AD1CON1bits.SSRC2 = 1;
		AD1CON1bits.CLRASAM = 1; // stop conversions
        AD1CON1bits.ASAM = 1;


        AD1CON2bits.SMPI = 15; // irq every 4 samples
		AD1CON2bits.CSCNA = 1; // scan inputs
        AD1CON2bits.VCFG = 0; //AVdd / AVss

        AD1CON3bits.SAMC = 31; // Sample time, TAD 31
		AD1CON3bits.ADCS = 64; // Conv time

        AD1CHS = 0; // Neg/pos select channel

		AD1CSSLbits.CSSL0 = 1; // Channel scan
		AD1CSSLbits.CSSL1 = 1; // Channel scan
		AD1CON1bits.ADON = 1;
        portNOP();
        //AD1CON1bits.SAMP = 1;
	}

    DMACONbits.ON = 1;
}
